using System;
using LBNHelpers;
using Microsoft.SPOT;
using MSH = Microsoft.SPOT.Hardware;

namespace NXTNetMF
{
	public class NxtMMX : IDisposable
	{
		private static class I2CRegisters
		{
			#region General purpose registers
			/// read the NxtMMX battery voltage (mV = value * 37)
			public const byte RO_BatteryVoltage = 0x41;

			/// Issue a command
			public const byte WO_Command = 0x41;
			#endregion

			#region Motor 1 R/W registers
			/// Get/set the encoder target for motor 1 (4 bytes length register)
			public const byte RW_Motor1_EncoderTarget = 0x42;

			/// Motor 1 speed ([-100;100])
			public const byte RW_Motor1_Speed = 0x46;

			/// Time to run for motor 1 (in second)
			public const byte RW_Motor1_TimeToRun = 0x47;

			/// Command register B for motor 1 (not used so far)
			public const byte RW_Motor1_CommandB = 0x48;

			/// Command register A for motor 1
			public const byte RW_Motor1_CommandA = 0x49;
			#endregion

			#region Motor 2 R/W registers
			/// Get/set the encoder target for motor 2 (4 bytes length register)
			public const byte RW_Motor2_EncoderTarget = 0x4A;

			/// Motor 2 speed ([-100;100])
			public const byte RW_Motor2_Speed = 0x4E;

			/// Time to run for motor 2 (in second)
			public const byte RW_Motor2_TimeToRun = 0x4F;

			/// Command register B for motor 2 (not used so far)
			public const byte RW_Motor2_CommandB = 0x50;

			/// Command register A for motor 2
			public const byte RW_Motor2_CommandA = 0x51;
			#endregion

			#region Motor 1 RO registers
			/// Motor 1 encoder position (4 bytes length register)
			public const byte RO_Motor1_EncoderPosition = 0x62;

			/// Motor 2 encoder position (4 bytes length register)
			public const byte RO_Motor2_EncoderPosition = 0x66;

			/// Status of Motor 1
			public const byte RO_Motor1_Status = 0x72;

			/// Status of Motor 2
			public const byte RO_Motor2_Status = 0x73;

			/// Tasks running for Motor 1
			public const byte RO_Motor1_TasksRunning = 0x76;

			/// Tasks running for Motor 2
			public const byte RO_Motor2_TasksRunning = 0x77;
			#endregion
		}

		private enum Commands : byte
		{
			#region General Purpose commands
			Reset							= (byte)'R',
			IssueForBothMotors				= (byte)'S',
			#endregion

			#region Motor stopping commands
			Motor1_Float_WhileStopping		= (byte)'a',
			Motor2_Float_WhileStopping		= (byte)'b',
			MotorBoth_Float_WhileStopping	= (byte)'c',

			Motor1_Brake_WhileStopping		= (byte)'A',
			Motor2_Brake_WhileStopping		= (byte)'B',
			MotorBoth_Brake_WhileStopping	= (byte)'C',
			#endregion

			#region Encoder reset commands
			Motor1_ResetEncoder				= (byte)'r',
			Motor2_Resetencoder				= (byte)'s'
			#endregion
		}

		[Flags]
		public enum ControlRegister : byte
		{
			Speed		= 0x01,
			Ramp		= 0x02,
			Relative	= 0x04,
			Encoder		= 0x08,
			Brake		= 0x10,
			On			= 0x20,
			Time		= 0x40,
			Go			= 0x80
		}

		internal I2CDevice Device;

		public NxtMMX(I2CBus bus, ushort address, int timeout=100)
		{
			Device = bus.CreateDevice(address, timeout);
		}

		public void Dispose()
		{
			if (Device != null)
			{
				Device.Dispose();
				Device = null;
			}
		}

		public float BatteryVoltage
		{ 
			get
			{
				byte data = Device.ReadPortByte(I2CRegisters.RO_BatteryVoltage);

				var V = ((float)(data * 37)) / 1000;

				return V;
			}
		}

		#region Low level control of motors

		private Motor _motor1;
		public Motor Motor1
		{
			get
			{
				if (_motor1 == null)
				{
					_motor1 = new Motor(this, 1);
				}
				return _motor1;
			}
		}

		private Motor _motor2;
		public Motor Motor2
		{
			get
			{
				if (_motor2 == null)
				{
					_motor2 = new Motor(this, 2);
				}
				return _motor2;
			}
		}

		public class Motor
		{
			private NxtMMX Owner = null;
			private int MotorID = 0;

			internal Motor(NxtMMX nxtMmx, int motorid)
			{
				Owner = nxtMmx;
				MotorID = motorid;
			}

			#region Raw Properties

			public long EncoderPosition
			{
				get
				{
					byte[] data = Owner.Device.ReadPort(GetRegister(I2CRegisters.RO_Motor1_EncoderPosition, MotorID), 4);
					long v = Convertion.From(data);
					return v;
				}
			}

			public byte Status
			{
				get
				{
					byte data = Owner.Device.ReadPortByte(GetRegister(I2CRegisters.RO_Motor1_Status, MotorID));
					return data;
				}
			}

			public byte TaskRunning
			{
				get
				{
					byte data = Owner.Device.ReadPortByte(GetRegister(I2CRegisters.RO_Motor1_TasksRunning, MotorID));
					return data;
				}
			}

			public long EncoderTarget
			{
				get
				{
					byte[] data = Owner.Device.ReadPort(GetRegister(I2CRegisters.RW_Motor1_EncoderTarget, MotorID), 4);
					long v = Convertion.From(data);
					return v;
				}

				set
				{
					var data = Convertion.To(value);
					Owner.Device.WritePort(GetRegister(I2CRegisters.RW_Motor1_EncoderTarget, MotorID), data);
				}
			}

			public byte Speed
			{
				get
				{
					byte data = Owner.Device.ReadPortByte(GetRegister(I2CRegisters.RW_Motor1_Speed, MotorID));
					return data;
				}

				set
				{
					Owner.Device.WritePort(GetRegister(I2CRegisters.RW_Motor1_Speed, MotorID), value);
				}
			}

			public byte TimeToRun
			{
				get
				{
					byte data = Owner.Device.ReadPortByte(GetRegister(I2CRegisters.RW_Motor1_TimeToRun, MotorID));
					return data;
				}

				set
				{
					Owner.Device.WritePort(GetRegister(I2CRegisters.RW_Motor1_TimeToRun, MotorID), value);
				}
			}

			public byte CommandA
			{
				get
				{
					byte data = Owner.Device.ReadPortByte(GetRegister(I2CRegisters.RW_Motor1_CommandA, MotorID));
					return data;
				}

				set
				{
					Owner.Device.WritePort(GetRegister(I2CRegisters.RW_Motor1_CommandA, MotorID), value);
				}
			}

			#endregion

			private static byte GetRegister(byte address, int motorId)
			{
				if (address < I2CRegisters.RW_Motor2_EncoderTarget)			return (byte)(address + ((motorId == 1) ? 0 : 8));

				if (address == I2CRegisters.RO_Motor1_EncoderPosition)		return (byte)(address + ((motorId == 1) ? 0 : 4));

				return (byte)(address + ((motorId == 1) ? 0 : 1));
			}
		}

		#endregion

		public bool RunUnlimited(WhichMotor which, MotorDirection dir, float speed)
		{
			const ControlRegister control = ControlRegister.Speed | ControlRegister.Go | ControlRegister.Brake | ControlRegister.On;
			var sspeed = ComputeByteSpeed(dir, speed);
			return SetSpeedTimeAndControl(which, sspeed, 0, control);
		}

		public bool MotorBothBrake()
		{
			return IssueCommand(Commands.MotorBoth_Brake_WhileStopping);
		}

		public bool MotorsReset()
		{
			return IssueCommand(Commands.Reset);
		}

		//public bool PowerRamping { get; set; }
		//public NextAction NextAction { get; set; }
		//public bool SpeedContrrtol { get; set; }


		#region private helpers

		public bool SetEncoderSpeedTimeAndControl(WhichMotor which, long encoder, byte speed, byte duration, ControlRegister control)
		{
			if (which == WhichMotor.MotorBoth)
			{
				control &= ~ControlRegister.Go;
				var m1 = SetEncoderSpeedTimeAndControl(WhichMotor.Motor1, encoder, speed, duration, control);
				var m2 = SetEncoderSpeedTimeAndControl(WhichMotor.Motor1, encoder, speed, duration, control);

				StartMotorInSync();

				return m1 && m2;
			}

			var data = new byte[8];
			Convertion.To(encoder, ref data);
			data[4] = speed;
			data[5] = duration;
			data[6] = 0;
			data[7] = (byte)control;

			var s = Device.WritePort((which == WhichMotor.Motor1) ? I2CRegisters.RW_Motor1_EncoderTarget : I2CRegisters.RW_Motor2_EncoderTarget, data);

			return (s == 9);		
		}

		public bool SetSpeedTimeAndControl(WhichMotor which, byte speed, byte duration, ControlRegister control)
		{
			if (which == WhichMotor.MotorBoth)
			{
				control &= ~ControlRegister.Go;
				var m1 = SetSpeedTimeAndControl(WhichMotor.Motor1, speed, duration, control);
				var m2 = SetSpeedTimeAndControl(WhichMotor.Motor1, speed, duration, control);

				StartMotorInSync();

				return m1 && m2;
			}

			var data = new byte[4];
			data[0] = speed;
			data[1] = duration;
			data[2] = 0;
			data[3] = (byte) control;

			var s = Device.WritePort((which == WhichMotor.Motor1) ? I2CRegisters.RW_Motor1_Speed : I2CRegisters.RW_Motor2_Speed, data);

			return (s == 5);
		}

		private void StartMotorInSync()
		{
			IssueCommand(Commands.IssueForBothMotors);
		}

		private bool IssueCommand(Commands cmd)
		{
			var s = Device.WritePort(I2CRegisters.WO_Command, (byte)cmd);
			return s == 2;
		}

		private static byte ComputeByteSpeed(MotorDirection dir, float speed)
		{
		//	speed = Maths.Clip(speed, 0, 1);

			byte b = (byte) (speed*100);

			return (dir == MotorDirection.Forward) ? b : (byte)-b;
		}
		#endregion
	}

	public enum WhichMotor
	{
		Motor1 = 1,
		Motor2,
		MotorBoth,
	}

	public enum MotorDirection
	{
		Forward = 1,
		Reverse
	}

	public enum MotorMode
	{
		Encoder = 1,
		Timed,
		Unlimited
	}

	public enum NextAction
	{
		Brake = 1,
		BrakeHold,
		Coast
	}
}
